from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取包的共享目录
    # pkg_share = get_package_share_directory('one_line_lidar_mapping')
    extr_calib_node = Node(
        package='extr_calib',
        executable='extr_calib',
        name='extr_calib_node',
        output='screen',
        remappings=[('/odom1', '/odometry/gps'),
                    ('/odom2', '/Odometry/transform')],  # 将lio对齐到gps上
        # remappings=[('/odom1', '/odometry/gps'),
        #             ('/odom2', '/Odometry')],
        parameters=[{
                'window_sec': 200.0,
        }]
    )

    utm_to_gps_odometry_node = Node(
        package='robot_odom_transform',
        executable='utm_to_gps_odometry_node',
        name='utm_to_gps_odometry_node',
        output='screen',
        parameters=[{
            "input_odom_topic": "utm/gps",  # 输入的UTM里程计话题
            "zeroed_odom_topic": "odometry/gps",  # 修正后的
            "initial_pose_topic": "utm/initial_pose",  # 初始定位姿态话题
            "zeroed_frame_id": "camera_init",  # 修正后的坐标系
            "publish_initial_pose": True,  # 是否发布初始定位姿态
            "init_frames": 10,  # 初始化零点所需采集的帧数
        }]
    )

    odom_transform_pub = Node(
        package='robot_odom_transform',
        executable='odom_transform_pub',

        name='odom_transform_pub',
        output='screen',
        remappings=[
            ('extrinsics', 'extrinsics'),  # 订阅的外参话题
            ('Odometry/transform', 'Odometry/transform'),  # 订阅的里程计话题
            ('odom_transformed', 'odom_transformed'),  # 发布转换后的里程计话题

        ]
    )

    # 返回启动描述
    return LaunchDescription([
        extr_calib_node,
        utm_to_gps_odometry_node,
        odom_transform_pub,
    ])
